#ifndef TF_CALIBRATION_H
#define TF_CALIBRATION_H

#include "blobDetection.h"

#include <ros/ros.h>

#include <opencv-2.3.1/opencv2/highgui/highgui.hpp>
#include <opencv-2.3.1/opencv2/imgproc/imgproc.hpp>

#include <tf/transform_broadcaster.h>

#include <cmath>

typedef struct colorSpans
{
	colorSpans()
	{
		colorHue = 0;
		colorSat = 128;
		colorSpan = 10;
		found = false;
	}

	int colorHue;
	int colorSat;
	int colorSpan;

	cv::Point3d point;
	cv::Point2d screenPoint;
	bool found;

}colorSpans;


typedef struct samplePoint
{
	cv::Point3d point;
	cv::Point2d point2d;
}samplePoint;

/******************************************************************
Function for convert to radians
****************************************************************/
inline float toRad(float deg)
{
	return (M_PI*deg) / 180;
}

#define NUMAXIS 3

class TfCalibration
{
public:
	TfCalibration(ros::NodeHandle& n);
	~TfCalibration();

	void ImageCallback(cv::Mat& image, const sensor_msgs::CameraInfoConstPtr &camInfo);
	void CalculateTransform();
	tf::StampedTransform RequestTransform();

	colorSpans colors[NUMAXIS];

private:
	// Blobtracker class
	BlobDetection *mBlobDetector[NUMAXIS];
	samplePoint mFixturePoints[NUMAXIS];

	bool mFound;
	cv::Point3d mOffset;
	cv::Point3d mRotation;
};

#endif // TF_CALIBRATION_H
